////////////////////////////////////////////////////////
////////////////////////////////////////////////////////
////    main.c 
////////////////////////////////////////////////////////
////////////////////////////////////////////////////////

#include <stdlib.h>
#include <string.h>
#include <math.h>

#include <avr/io.h>
#include <avr/pgmspace.h>

/* Pololu include files. */
#include <pololu/orangutan.h>

/* Scheduler include files. */
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"

/* i2c Interface include file. */
#include "i2cmaster.h"

/* Define the addresses of the devices that we want to manage on i2c bus.*/
#define SRF10 0xE0      // device address of SRF10 is 0xE0, see datasheet
#define TPA81 0xD0      // device address of TPA81 is 0xD2, see datasheet
/*-----------------------------------------------------------*/

static void TaskBlinkGreenLED(void *pvParameters); // Main Arduino (Green) LED Blink
static void TaskBlinkRedLED(void *pvParameters);   // Secondary (Red) LED Blink

static void TaskReadADC0(void *pvParameters);   // Read ADC0 Value
static void TaskReadI2C(void *pvParameters);   // Read I2C Bus & Write to LCD

static void TaskWriteAnalogLCD(void *pvParameters); // Write ADC0 to LCD

/*
 * Declare a variable of type xQueueHandle.
 * One queue will have the ADC value written to it.
 */
xQueueHandle xADCQueue;

/* Create a Semaphore mutex flag for the i2c Bus. To ensure only single access. */
xSemaphoreHandle xI2CSemaphore;

/* Create a Semaphore mutex flag for the LCD. To ensure only single access. */
xSemaphoreHandle xLCDSemaphore;

/*-----------------------------------------------------------*/

/* Main program loop */

int main(void)
{

	xADCQueue = xQueueCreate(2, sizeof(unsigned int));  // queue for ADC0 values

	xI2CSemaphore = xSemaphoreCreateMutex(); // mutex semaphore for i2c bus
	xLCDSemaphore = xSemaphoreCreateMutex(); // mutex semaphore for LCD

	clear();			// clear the LCD, move cursor to start of top line

	set_digital_output(IO_B5, LOW); // initialise Main Arduino (green IO_C4) LED
	set_digital_output(IO_D3, LOW); // initialise Red LED

	set_analog_mode(MODE_8_BIT);    // 8-bit analog-to-digital conversions
	start_analog_conversion(0);     // start initial ADC conversion

	xTaskCreate(TaskBlinkGreenLED, (const signed portCHAR *)"GreenLED", 128,
			NULL, 3, NULL);

	xTaskCreate(TaskBlinkRedLED, (const signed portCHAR *)"RedLED", 128, NULL,
			1, NULL);

	xTaskCreate(TaskReadADC0, (const signed portCHAR *)"ReadADCO", 128, NULL, 1,
			NULL);

	xTaskCreate(TaskReadI2C, (const signed portCHAR *)"ReadI2C", 128, NULL, 2,
			NULL);

	xTaskCreate(TaskWriteAnalogLCD, (const signed portCHAR *)"WriteAnalogLCD",
			128, NULL, 1, NULL);

	vTaskStartScheduler();

	//and finally, to eliminate compiler warnings....
	return (0);
}

/*-----------------------------------------------------------*/

static void TaskBlinkGreenLED(void *pvParameters) // Main Arduino (Green) LED Flash
{
	(void) pvParameters;
	;
	portTickType xLastWakeTime;
	/* The xLastWakeTime variable needs to be initialized with the current tick
	 count.  Note that this is the only time we access this variable.  From this
	 point on xLastWakeTime is managed automatically by the vTaskDelayUntil()
	 API function. */
	xLastWakeTime = xTaskGetTickCount();

	while (1)
	{
		set_digital_output(IO_B5, 0);            // main (green IO_C4) LED off
		vTaskDelayUntil(&xLastWakeTime, (200 / portTICK_RATE_MS));

		set_digital_output(IO_B5, 1);             // main (green IO_C4) LED on
		vTaskDelayUntil(&xLastWakeTime, (200 / portTICK_RATE_MS));
	}
}

static void TaskBlinkRedLED(void *pvParameters) // Red LED Flash
{
	(void) pvParameters;
	;
	portTickType xLastWakeTime;
	/* The xLastWakeTime variable needs to be initialized with the current tick
	 count.  Note that this is the only time we access this variable.  From this
	 point on xLastWakeTime is managed automatically by the vTaskDelayUntil()
	 API function. */
	xLastWakeTime = xTaskGetTickCount();

	while (1)
	{
		set_digital_output(IO_D3, 0);               // red LED off
		vTaskDelayUntil(&xLastWakeTime, (1000 / portTICK_RATE_MS));

		set_digital_output(IO_D3, 1);               // red LED on
		vTaskDelayUntil(&xLastWakeTime, (1000 / portTICK_RATE_MS));
	}
}

static void TaskReadADC0(void *pvParameters) // Read ADC0
{
	(void) pvParameters;
	;
	portTickType xLastWakeTime;
	/* The xLastWakeTime variable needs to be initialized with the current tick
	 count.  Note that this is the only time we access this variable.  From this
	 point on xLastWakeTime is managed automatically by the vTaskDelayUntil()
	 API function. */
	xLastWakeTime = xTaskGetTickCount();

	/*
	 * Variables for the analogue conversion on ADC0
	 */
	unsigned int sum = 0;
	unsigned int avg = 0;
	unsigned char samples = 0;

	while (1)
	{

		if (!analog_is_converting())     // if conversion is done...
		{
			sum += analog_conversion_result();  // get result
			start_analog_conversion(0);   // start next conversion

			if (++samples == 10)           // if 10 samples have been taken...
			{
				avg = sum / 10;       // compute 10-sample average of ADC result
				samples = 0;
				sum = 0;
			}
		}

		if (xADCQueue != 0)
		{
			// Send an unsigned int.  Wait for 10 ticks for space to become
			// available if necessary.
			xQueueSendToBack(xADCQueue, (void * ) &avg, (portTickType ) 10);
		}

		vTaskDelayUntil(&xLastWakeTime, (100 / portTICK_RATE_MS));

	}

}

static void TaskWriteAnalogLCD(void *pvParameters) // Write to LCD
{
	(void) pvParameters;
	;

	unsigned int adc;    // variable to hold the average ADC value off the queue

	while (1)
	{
		if (xLCDSemaphore != NULL )
		{
			// See if we can obtain the semaphore.  If the semaphore is not available
			// wait 10 ticks to see if it becomes free.
			if (xSemaphoreTake( xLCDSemaphore, ( portTickType ) 10 ) == pdTRUE)
			{
				// We were able to obtain the semaphore and can now access the
				// shared resource.
				if (xADCQueue != 0)
				{
					/* Block on the queue to wait for data to arrive. */
					if (xQueueReceive( xADCQueue, &adc, portMAX_DELAY))
					{
						lcd_goto_xy(4, 2); // go to the fifth character of the third LCD line
						print_long(adc);         // print the averaged ADC value
						print(" ADC   "); // added spaces are to overwrite left over chars
						lcd_goto_xy(4, 3); // LCD cursor to fifth character of the forth line
						print_long(
								(portLONG) ((portFLOAT) adc * 3300.0 / 255.0)); // print mV converted
//                      print_long((portLONG)(34505.0 * pow((portFLOAT)adc,-1.1908)));                       
						print(" mV    "); // added spaces are to overwrite left over chars

					}
				}
				xSemaphoreGive(xLCDSemaphore);
			}
		}
	}
}

/*-----------------------------------------------------------*/

static void TaskReadI2C(void *pvParameters) // Read i2c Bus
{
	(void) pvParameters;
	;

	portTickType xLastWakeTime;
	/* The xLastWakeTime variable needs to be initialized with the current tick
	 count.  Note that this is the only time we access this variable.  From this
	 point on xLastWakeTime is managed automatically by the vTaskDelayUntil()
	 API function. */
	xLastWakeTime = xTaskGetTickCount();

	/*
	 * Variable for the averaging
	 */
	unsigned char samples = 0;
	unsigned char ret; // Holds return from the Thermopile TPA81.

	unsigned char ret0 = 0; // Holds return value 0 from the Thermopile TPA81.
	unsigned char ret1 = 0; // Holds return value 1 from the Thermopile TPA81.
	unsigned char ret2 = 0; // Holds return value 2 from the Thermopile TPA81.
	unsigned char ret3 = 0; // Holds return value 3 from the Thermopile TPA81.
	unsigned char ret4 = 0; // Holds return value 4 from the Thermopile TPA81.
	unsigned char ret5 = 0; // Holds return value 5 from the Thermopile TPA81.
	unsigned char ret6 = 0; // Holds return value 6 from the Thermopile TPA81.
	unsigned char ret7 = 0; // Holds return value 7 from the Thermopile TPA81.
	unsigned char ret8 = 0; // Holds return value 8 from the Thermopile TPA81.

	while (1)
	{
		if (xI2CSemaphore != NULL )
		{
			// See if we can obtain the semaphore.  If the semaphore is not available
			// wait 10 ticks to see if it becomes free.
			if (xSemaphoreTake( xI2CSemaphore, ( portTickType ) 10 ) == pdTRUE)
			{
// We were able to obtain the semaphore and can now access the
// shared resource.

				/*  Reading from the Slave
				 Before reading data from the slave device, you must tell it which of its internal addresses you want to read. So a read of the slave actually starts off by writing to it. This is the same as when you want to write to it: You send the start sequence, the I2C address of the slave with the R/W bit low (even address) and the internal register number you want to write to. Now you send another start sequence (sometimes called a restart) and the I2C address again - this time with the read bit set. You then read as many data bytes as you wish and terminate the transaction with a stop sequence. So to read the compass bearing as a byte from the TPA81 module:

				 1. Send a start sequence
				 2. Send 0xD0 ( I2C address of the TPA81 with the R/W bit low (even address)
				 3. Send 0x00 (Internal address of the bearing register)
				 4. Send a start sequence again (repeated start)
				 5. Send 0xD1 ( I2C address of the TPA81 with the R/W bit high (odd address)
				 6. Read data byte from TPA81
				 7. Send the stop sequence.
				 */
				i2c_init();              // init I2C interface
				do
				{
					ret = i2c_start(TPA81 + I2C_WRITE); // set device address and write mode

					if (ret)
					{
						/* failed to issue start condition, possibly no device found */
						i2c_stop();
						set_digital_output(IO_D3, 1); // red LED on to show error */

					}
					else
					{
						/* issuing start condition ok, device accessible */
						i2c_write(0x01);          // write address = 1 (Ambient)

						i2c_rep_start(TPA81 + I2C_READ); // set device address and read mode
						ret0 += i2c_readAck(); // read one byte from address 0
						ret1 += i2c_readAck();   // read one byte from address 1
						ret2 += i2c_readAck();   // read one byte from address 2
						ret3 += i2c_readAck();   // read one byte from address 3
						ret4 += i2c_readAck();   // read one byte from address 4
						ret5 += i2c_readAck();   // read one byte from address 5
						ret6 += i2c_readAck();   // read one byte from address 6
						ret7 += i2c_readAck();   // read one byte from address 7
						ret8 += i2c_readNak();   // read one byte from address 8
						i2c_stop();          // set stop condition = release bus
					}

				} while (++samples < 5); // Averaging the result over 5 samples.

				ret0 /= samples;                // Averaging calculations.
				ret1 /= samples;
				ret2 /= samples;
				ret3 /= samples;
				ret4 /= samples;
				ret5 /= samples;
				ret6 /= samples;
				ret7 /= samples;
				ret8 /= samples;

			}
		}

		// We have finished accessing the shared resource.
		// Release the i2c semaphore.
		xSemaphoreGive(xI2CSemaphore);

		// output results on the LCD

		if (xLCDSemaphore != NULL )
		{
			// See if we can obtain the semaphore.  If the semaphore is not available
			// wait 10 ticks to see if it becomes free.
			if (xSemaphoreTake( xLCDSemaphore, ( portTickType ) 10 ) == pdTRUE)
			{
				// We were able to obtain the semaphore and can now access the
				// shared resource.

				lcd_goto_xy(0, 0);// LCD cursor to first character of the first LCD line
				print("<");	// added spaces are to overwrite left over chars
				print_long((long) ret8);          // print the temperature value
				print(" ");
				print_long((long) ret7);          // print the temperature value
				print(" ");
				print_long((long) ret6);          // print the temperature value
				print(" ");
				print_long((long) ret5);          // print the temperature value
				print(" Ambn ");
				print_long((long) ret0);  // print the ambient temperature value

				lcd_goto_xy(1, 1); // go to the second character of the second LCD line
				print_long((long) ret4);          // print the temperature value
				print(" ");
				print_long((long) ret3);          // print the temperature value
				print(" ");
				print_long((long) ret2);          // print the temperature value
				print(" ");
				print_long((long) ret1);          // print the temperature value
				print("> from ");
				print_long((long) samples);       // print the number of samples

				// Reset our variable for new averaging.
				ret0 = ret1 = ret2 = ret3 = ret4 = ret5 = ret6 = ret7 = ret8 =
						0;
				samples = 0;

				// We have finished accessing the shared resource.
				// Release the LCD semaphore.
				xSemaphoreGive(xLCDSemaphore);
			}
		}

		vTaskDelayUntil(&xLastWakeTime, (400 / portTICK_RATE_MS));
	}
}
/*-----------------------------------------------------------*/

